import multiprocessing
import time
import serial
import ctypes as ct


def assamble(xL, xH):
    xH = int(xH, 16)
    xL = int(xL, 16)
    return ct.c_short((xH << 8) | xL).value


def transform_acc(acc):
    return float(acc) / 32768 * 4


def transform_gyro(gyro):
    return float(gyro) / 32768 * 2000


def imu_process(queue):
    """
    帧格式:
    帧头        ID     个数N    DATA       SUM
    0x55 0x55   0x03   12      N字节数据   校验和
    数据格式：
    5   6   7   8   9   10  11  12  13  14  15  16
    AxL AxH AyL AyH AzL AzH GxL GxH GyL GyH GzL GzH

    加速度计原始数据
    Ax = (int16_t) (AxH<<8) | AxL
    转换为加速度(单位 G)
    ACCx = (float) Ax / 32768 * 4, 4-用户设置的量程G; 1G 表示9.8m/s2

    陀螺仪原始数据
    Gx= (int16_t) (GxH<<8) | GxL, 换为旋转速率(单位°/S)
    Gyrox = (float) Gx / 32768 * 2000, 2000-用户设置的量程单位°/S
    """
    with open("./raw_data.csv", "w") as raw, open("./hex_data.csv", "w") as hex_data:
        raw.writelines("Ax,Ay,Az,Gx,Gy,Gz\n")
        while True:
            data = queue.get()
            Ax = assamble(data[8:10], data[10:12])
            Ay = assamble(data[12:14], data[14:16])
            Az = assamble(data[16:18], data[18:20])

            Acc = [transform_acc(Ax), transform_acc(Ay), transform_acc(Az)]

            Gx = assamble(data[20:22], data[22:24])
            Gy = assamble(data[24:26], data[26:28])
            Gz = assamble(data[28:30], data[30:32])

            Gyro = [transform_gyro(Gx), transform_gyro(Gy), transform_gyro(Gz)]

            raw.writelines(",".join(str(i) for i in (Acc + Gyro)) + "\n")
            hex_data.writelines(data + "\n")


def comfirm(data):
    """最后一个字节 等于 之前的所有字节之和"""
    sum = 0
    for i in range(16):
        sum += int(data[2 * i : 2 * i + 2], 16)
    if (sum & 0xFF) == int(data[32:34], 16):
        return True
    return False


def imu_read(queue):
    com = serial.Serial("COM4", 460800)
    data = ""
    while True:
        data += com.read().hex()
        if len(data) < 34:
            continue

        if data.startswith("5555030c"):
            if comfirm(data):
                queue.put(data)
                data = ""
                continue
            else:
                # 把头移调
                data = data[4:]

        start = data.find("5555")
        if start != -1:
            data = data[start:]
        else:
            data = ""


if __name__ == "__main__":
    q = multiprocessing.Queue()
    p = multiprocessing.Process(target=imu_process, args=(q,))
    p.start()
    imu_read(q)
